#include "motor.h"

uint8_t motor_txbuff[32] = {0};
uint8_t motor_rxbuff[32] = {0};
int Encoder_Offset[4];
int Encoder_Now[4];
extern uint8_t car_mode;
int start_encoder = 0;
int target_encoder = 0;
uint8_t started = 0;
void IIC_Send_Byte(uint8_t txd) //hi2c2
{
    HAL_I2C_Master_Transmit(&hi2c2, 0xA0, &txd, 1, 10); // 0xA0为写地址
}

int i2cWrite(uint8_t addr, uint8_t reg, uint8_t len, uint8_t *data)
{
    uint8_t txbuf[32] = {0};
    txbuf[0] = reg; // 寄存器地址
    for (int i = 0; i < len; i++)
    {
        txbuf[i + 1] = data[i];
    }
    HAL_I2C_Master_Transmit(&hi2c2, addr, txbuf, len + 1, 10);
    return 0;
}


int i2cRead(uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf)
{
    // 推荐用 HAL_I2C_Mem_Read，保证时序正确
    HAL_I2C_Mem_Read(&hi2c2, addr, reg, I2C_MEMADD_SIZE_8BIT, buf, len, 10);
    return 0;
}

uint8_t IIC_Read_Byte(unsigned char ack)
{
    uint8_t data = 0;
    if (ack)
    {
        HAL_I2C_Master_Receive(&hi2c2, 0xA1, &data, 1, 10); // 0xA1为读地址
    }
    else
    {
        HAL_I2C_Master_Receive(&hi2c2, 0xA1, &data, 1, 10); // 0xA1为读地址
    }
    return data;
}

  
//浮点型转bytes位 //bytes 4个长度 因为float是4字节	Convert float to bytes //bytes 4 in length because float is 4 bytes
void float_to_bytes(float f, uint8_t *bytes) 
{
    memcpy(bytes, &f, sizeof(float));
}

//bytes位转成浮点型	Convert bytes to floating point
float char2float(char *p)
{
  float *p_Int;
  p_Int = (float *)malloc(sizeof(float));
  memcpy(p_Int, p, sizeof(float));
  float x = *p_Int;
  free(p_Int);
  return x;
}


//配置电机	Configure the motor
void Set_motor_type(uint8_t data)
{	
	i2cWrite(Motor_model_ADDR,MOTOR_TYPE_REG,2,&data);
}

//配置死区	Configuring Dead Zone
void Set_motor_deadzone(uint16_t data)
{
	static uint8_t buf_tempzone[2];
	
	buf_tempzone[0] = (data>>8)&0xff;
	buf_tempzone[1] = data;
	
	i2cWrite(Motor_model_ADDR,MOTOR_DeadZONE_REG,2,buf_tempzone);
}

//配置磁环线	Configuring magnetic loop
void Set_Pluse_line(uint16_t data)
{
	static uint8_t buf_templine[2];
	
	buf_templine[0] = (data>>8)&0xff;
	buf_templine[1] = data;
	
	i2cWrite(Motor_model_ADDR,MOTOR_PluseLine_REG,2,buf_templine);
}

//配置减速比	Configure the reduction ratio
void Set_Pluse_Phase(uint16_t data)
{
	static uint8_t buf_tempPhase[2];
	
	buf_tempPhase[0] = (data>>8)&0xff;
	buf_tempPhase[1] = data;
	
	i2cWrite(Motor_model_ADDR,MOTOR_PlusePhase_REG,2,buf_tempPhase);
}


//配置直径	Configuration Diameter
void Set_Wheel_dis(float data)
{
	static uint8_t bytes[4];
	
	float_to_bytes(data,bytes);
	
	i2cWrite(Motor_model_ADDR,WHEEL_DIA_REG,4,bytes);
}





void control_speed(int16_t m1,int16_t m2 ,int16_t m3,int16_t m4)
{
	static uint8_t speed[8];
	
	speed[0] = (m1>>8)&0xff;
	speed[1] = (m1)&0xff;
	
	speed[2] = (m2>>8)&0xff;
	speed[3] = (m2)&0xff;
	
	speed[4] = (m3>>8)&0xff;
	speed[5] = (m3)&0xff;
	
	speed[6] = (m4>>8)&0xff;
	speed[7] = (m4)&0xff;
	i2cWrite(Motor_model_ADDR,SPEED_Control_REG,8,speed);
}

void my_set_speed(int16_t l_speed,int16_t r_speed)
{
    int16_t m1 = l_speed;
    int16_t m4 = r_speed;

    // 通过控制速度函数设置电机速度
    control_speed(m1, 0, 0, m4);
}


void control_pwm(int16_t m1,int16_t m2 ,int16_t m3,int16_t m4)
{
	static uint8_t pwm[8];
	
	pwm[0] = (m1>>8)&0xff;
	pwm[1] = (m1)&0xff;
	
	pwm[2] = (m2>>8)&0xff;
	pwm[3] = (m2)&0xff;
	
	pwm[4] = (m3>>8)&0xff;
	pwm[5] = (m3)&0xff;
	
	pwm[6] = (m4>>8)&0xff;
	pwm[7] = (m4)&0xff;
	
	i2cWrite(Motor_model_ADDR,PWM_Control_REG,8,pwm);

}


void Read_10_Enconder(void) //10ms
{
    static uint8_t buf[2];
		
	//M1电机编码器的数据	M1 motor encoder data
	i2cRead(Motor_model_ADDR, READ_TEN_M1Enconer_REG, 2, buf);
	Encoder_Offset[0] = (int16_t)((buf[0]<<8)|buf[1]);
	
	//M2电机编码器的数据	M2 motor encoder data
	i2cRead(Motor_model_ADDR, READ_TEN_M2Enconer_REG, 2, buf);
	Encoder_Offset[1] = (int16_t)((buf[0]<<8)|buf[1]);
	
	//M3电机编码器的数据	M3 motor encoder data
	i2cRead(Motor_model_ADDR, READ_TEN_M3Enconer_REG, 2, buf);
	Encoder_Offset[2] = (int16_t)((buf[0]<<8)|buf[1]);
	
	//M4电机编码器的数据	M4 motor encoder data
	i2cRead(Motor_model_ADDR, READ_TEN_M4Enconer_REG, 2, buf);
	Encoder_Offset[3] = (int16_t)((buf[0]<<8)|buf[1]);
	
}

void Read_ALL_Enconder(void)
{
	static uint8_t buf[2];
	static uint8_t buf2[2];
	
	//M1电机编码器的数据	M1 motor encoder data
	i2cRead(Motor_model_ADDR, READ_ALLHigh_M1_REG, 2, buf);
	i2cRead(Motor_model_ADDR, READ_ALLLOW_M1_REG, 2, buf2);
	Encoder_Now[0] = buf[0]<<24|buf[1]<<16|buf2[0]<<8|buf2[1]; 
	
	//M2电机编码器的数据	M2 motor encoder data
	i2cRead(Motor_model_ADDR, READ_ALLHigh_M2_REG, 2, buf);
	i2cRead(Motor_model_ADDR, READ_ALLLOW_M2_REG, 2, buf2);
	Encoder_Now[1] = buf[0]<<24|buf[1]<<16|buf2[0]<<8|buf2[1];
	
	//M3电机编码器的数据	M3 motor encoder data
	i2cRead(Motor_model_ADDR, READ_ALLHigh_M3_REG, 2, buf);
	i2cRead(Motor_model_ADDR, READ_ALLLOW_M3_REG, 2, buf2);
	Encoder_Now[2] = buf[0]<<24|buf[1]<<16|buf2[0]<<8|buf2[1];
	
	
	//M4电机编码器的数据	M4 motor encoder data
	i2cRead(Motor_model_ADDR, READ_ALLHigh_M4_REG, 2, buf);
	i2cRead(Motor_model_ADDR, READ_ALLLOW_M4_REG, 2, buf2);
	Encoder_Now[3] = buf[0]<<24|buf[1]<<16|buf2[0]<<8|buf2[1];
	
}




// Motor_Version:1.7.3         // 电机驱动固件/硬件版本号
// Motor_type:5                // 电机类型（如1代表某种型号或编码方式）
// Dead_Zone:1600              // 死区值，低于此PWM/速度时电机不动作（消除死区影响）
// Pulse_Line:500              // 编码器每圈脉冲数（磁环线数）
// Pulse_Phase:28              // 减速比相关参数
// wheel_diameter:67.000       // 轮子直径，单位mm
// P:6.500	 I:0.400	 D:2.000	 // PID控制器的比例、积分、微分参数 
void motor_init(void)
{
    Set_motor_type(5); // 设置电机类型
    Set_motor_deadzone(1600); // 设置死区值
    Set_Pluse_line(500); // 设置编码器每圈脉冲数
    Set_Pluse_Phase(28); // 设置减速比相关参数
    Set_Wheel_dis(67.000); // 设置轮子直径
    control_speed(0, 0, 0, 0); // 初始化电机速度为0
}

void motor_proc(void)
{
    //__disable_irq(); // 禁用中断，防止在读取编码器时发生中断
    Read_ALL_Enconder(); // 读取总路程
    //__enable_irq(); // 重新启用中断
}

/**
 * @brief  根据行走长度（单位：厘米）计算编码器增加量
 * @param  distance_cm 行走距离，单位厘米
 * @return 编码器脉冲数
 */
int distance_to_encoder(float distance_cm)
{
    // 轮子直径，单位mm
    float wheel_diameter_mm = 67.0f;
    // 编码器每圈脉冲数（线数）
    int encoder_lines = 500;
    // 4倍频计数
    int pulse_per_motor_revolution = encoder_lines * 4;
    // 减速比
    float gear_ratio = 28.0f;
    
    // 计算轮子周长，单位mm
    float wheel_circumference_mm = 3.1415926f * wheel_diameter_mm;
    
    // 输入距离转为mm
    float distance_mm = distance_cm * 10.0f;
    
    // 计算轮子转数
    float wheel_revolutions = distance_mm / wheel_circumference_mm;
    
    // 计算电机转数（考虑减速比）
    float motor_revolutions = wheel_revolutions * gear_ratio;
    
    // 计算编码器脉冲数
    int encoder_count = (int)(motor_revolutions * pulse_per_motor_revolution + 0.5f);
    
    return encoder_count;
}
int get_Nowencoder_value(void)
{
    // 计算当前编码器值
    __disable_irq();
    Read_ALL_Enconder(); // 确保编码器值是最新的
    int now_encoder = (Encoder_Now[0] + Encoder_Now[3]) / 2; // 取左右编码器平均值
    __enable_irq(); // 重新启用中断
    return now_encoder; // 返回左右编码器平均值
}

// void go_straight_cm(int cm)
// {
//     //printf("started = %d\n ,nowencoder = %d , target_encoder = %d\n", started, (Encoder_Now[0] + Encoder_Now[3]) / 2, target_encoder);
//     //static float zero_yaw = Yaw; // 用于记录初始偏航角
    
//     if (started == 0)
//     {
//         __disable_irq(); // 禁用中断，防止在读取编码器时发生中断
//         Read_ALL_Enconder(); // 获取初始编码器值
//         // 取左右编码器平均值作为起点
//         start_encoder = (Encoder_Now[0] + Encoder_Now[3]) / 2;
//         __enable_irq(); // 重新启用中断
//         target_encoder = start_encoder + distance_to_encoder(cm); // 目标编码器值
//         started = 1;
//         my_set_speed(200, 200); // 设置初始速度

//     } 
//     else if (started == 1)
//     {
//         //关中断
//         __disable_irq(); // 禁用中断，防止在读取编码器时发生中断
//         Read_ALL_Enconder(); // 持续读取编码器
//         int now_encoder = (Encoder_Now[0] + Encoder_Now[3]) / 2;
//         //开启中断
//         __enable_irq(); // 重新启用中断
//         if (now_encoder >= target_encoder) {
//             my_set_speed(0, 0); // 到达目标，停止
//             //printf("target enc = %d\n", target_encoder);
//             //printf("end!\n");
//             car_mode = 4;

//         } 
//         else if (now_encoder < target_encoder) {
//             // 角度环修正量
//             float angle_correction = PID_Calc(&Yaw_PID,0,Yaw,1);
//             float lspeed = 200 - angle_correction; // 左轮速度
//             float rspeed = 200 + angle_correction; // 右轮速度
//             my_set_speed(200, 200); // 未到达目标，继续前进
//             //printf("no end!   LNOW:%d TARGET%d\n", Encoder_Now[0], target_encoder);

//         }

//     }
// }
void go_straight_cm(int cm)
{
    if (started == 0)
    {
        __disable_irq(); // 禁用中断，防止在读取编码器时发生中断
        Read_ALL_Enconder(); // 获取初始编码器值
        // 取左右编码器平均值作为起点
        start_encoder = (Encoder_Now[0] + Encoder_Now[3]) / 2;
        __enable_irq(); // 重新启用中断
        target_encoder = start_encoder + distance_to_encoder(cm); // 目标编码器值
        started = 1;
        my_set_speed(200, 200); // 设置初始速度
    } 
    else if (started == 1)
    {
        //关中断
        HAL_NVIC_DisableIRQ(TIM6_DAC_IRQn); // 禁用TIM6中断
        Read_ALL_Enconder(); // 持续读取编码器
        int now_encoder = (Encoder_Now[0] + Encoder_Now[3]) / 2;
        //开启中断
        HAL_NVIC_EnableIRQ(TIM6_DAC_IRQn); // 重新启用TIM
        if (now_encoder >= target_encoder) {
            my_set_speed(0, 0); // 到达目标，停止
            //printf("target enc = %d\n", target_encoder);;
            car_mode = 2;
            started = 0; // 重置状态，准备下一次调用
        } 
        else if (now_encoder < target_encoder) {
            // 角度环修正量
            float angle_correction = PID_Calc(&Yaw_PID,0,Yaw,1);
            float lspeed = 200 - angle_correction; // 左轮速度
            float rspeed = 200 + angle_correction; // 右轮速度
            my_set_speed(200, 200); // 未到达目标，继续前进
            printf("no end!   LNOW:%d TARGET%d\n", Encoder_Now[0], target_encoder);
        }
    }
}